#include "adjust.h"
adjust::adjust(/* args */)
{
    std::string filename;
    ros::NodeHandle phc("~");
    phc.getParam("/dis_cor/preprocess_config", filename);
    std::cout << "filename:" << filename << std::endl;
    std::map<string, string> m;
    // readconfig_.ReadConfig(filename, m);
    // ReadProConfig(m, proprecess_config_);
}

adjust::~adjust()
{
}

CloudData::CLOUD adjust::AdjustPoints3(CloudData::CLOUD_PTR lidar_imu_, CloudData::CLOUD output, ImuData imu_data, double time)
{
    if (!output.points.empty())
    {
        output.points.clear();
    }

    CloudData::CLOUD_PTR origin_point = lidar_imu_;
    float orientation_space = 2.0 * M_PI;
    // float delete_space = 5.0 * M_PI / 180.0;
    float start_orientation = -M_PI;
    double time1 = ros::Time::now().toSec();
    for (size_t point_index = 1; point_index < origin_point->points.size(); ++point_index)
    {
        float orientation = std::atan2(origin_point->points[point_index].y, origin_point->points[point_index].x) + start_orientation;
        // orientation = M_PI - orientation;
        if (orientation < 0.0)
            orientation += 2.0 * M_PI;

        // if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space)
        //     continue;
        // sensor_msgs/PointCloud2 发送的时间是最后发送topic的时间 ，即最后时刻, real_time 起点时间到当前点的时间
        // 但是为了把起点的所有点都转到终点时刻，所以减去一帧的时间,即以终点时刻为原点坐标系进行运动补偿
        float ratio = fabs(orientation) / orientation_space;
        float real_time = fabs(orientation) / orientation_space * scan_period_;
        float x_offset = imu_data.LinerX * real_time;
        float y_offset = imu_data.LinerY * real_time;
        float z_offset = imu_data.LinerZ * real_time;
        discor_param[3] = -x_offset;
        discor_param[7] = -y_offset;
        discor_param[11] = -z_offset;
        if (imu_data.AngZ > 2 * M_PI || imu_data.AngZ < -2 * M_PI)
            imu_data.AngZ = 0;
        if (imu_data.AngY > 2 * M_PI || imu_data.AngY < -2 * M_PI)
            imu_data.AngY = 0;
        if (imu_data.AngX > 2 * M_PI || imu_data.AngX < -2 * M_PI)
            imu_data.AngX = 0;
        double cos_yaw = std::cos(imu_data.AngZ * ratio * degree2arc);
        double sin_yaw = std::sin(imu_data.AngZ * ratio * degree2arc);
        double cos_pit = std::cos(imu_data.AngY * ratio * degree2arc);
        double sin_pit = std::sin(imu_data.AngY * ratio * degree2arc);
        double cos_rol = std::cos(imu_data.AngX * ratio * degree2arc);
        double sin_rol = std::sin(imu_data.AngX * ratio * degree2arc);

        discor_param[0] = cos_yaw * cos_pit;
        discor_param[1] = cos_yaw * sin_pit * sin_rol - sin_yaw * cos_rol;
        discor_param[2] = cos_yaw * sin_pit * cos_rol + sin_yaw * sin_rol;
        discor_param[4] = sin_yaw * cos_pit;
        discor_param[5] = sin_yaw * sin_pit * sin_rol + cos_yaw * cos_rol;
        discor_param[6] = sin_yaw * sin_pit * cos_rol - cos_yaw * sin_rol;
        discor_param[8] = -sin_pit;
        discor_param[9] = cos_pit * sin_rol;
        discor_param[10] = cos_pit * cos_rol;

        xyz point;
        float x = discor_param[0] * origin_point->points[point_index].x +
                  discor_param[1] * origin_point->points[point_index].y +
                  discor_param[2] * origin_point->points[point_index].z +
                  discor_param[3];
        float y = discor_param[4 + 0] * origin_point->points[point_index].x +
                  discor_param[4 + 1] * origin_point->points[point_index].y +
                  discor_param[4 + 2] * origin_point->points[point_index].z +
                  discor_param[4 + 3];
        float z = discor_param[8 + 0] * origin_point->points[point_index].x +
                  discor_param[8 + 1] * origin_point->points[point_index].y +
                  discor_param[8 + 2] * origin_point->points[point_index].z +
                  discor_param[8 + 3];
        point.x = x;
        point.y = y;
        point.z = z;
        output.points.push_back(point);
    }
    return output;
}
Eigen::Matrix3f adjust::UpdateMatrix(float real_time)
{
    Eigen::Vector3f angle = angular_rate_ * real_time;
    Eigen::AngleAxisf t_Vz(angle(2), Eigen::Vector3f::UnitZ());
    Eigen::AngleAxisf t_Vy(angle(1), Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf t_Vx(angle(0), Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf t_V;
    t_V = t_Vz * t_Vy * t_Vx;
    return t_V.matrix();
}
